/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "modbuscom.h"
#include "modbus485driver.h"
#include "modbusiomoduledriver.h"
#include "qdebug.h"
#include "messagelog.h"
#include "robotcontrol.h"
//#include "robotbase.h"
#include "timetool.h"
#include "vrepobject.h"
#include "vrepcontrol.h"
#include <QThread>
#include "modbusdriverconfig.h"
#include "ecatbusmanager.h"

#define D_TRY_MAX_COUNT 5


extern RobotControl* robCtl;

ModbusCom::ModbusCom(E_MODBUS_TYPE busTypeIn, int busIdIn
                     , QList<QString> xmlFilelist, QList<int> deviceTypeList, QList<int> aliasList,
                     int pthread_priority_in, int pthread_schedule_in)
            :EcatMasterBase(pthread_priority_in,pthread_schedule_in)
{

    busType=busTypeIn;
    busId=busIdIn;
    currentAlias=-1;
    currentModbusMaster=NULL;

    QString rootFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString filePath;
    if(E_MODBUS_TYPE_RTU==busTypeIn)
    {
        filePath=rootFilePath+GLOBAL_MODBUS_RTU_PATH+"checkConfig.ini";
    }
    else if(E_MODBUS_TYPE_TCP==busTypeIn)
    {
        filePath=rootFilePath+GLOBAL_MODBUS_TCP_PATH+"checkConfig.ini";
    }
    int tmpReturn=readCheckConfig(filePath,wkcLostLimit,ecatPeriodTimerOffsetMax,
                    ecatReceiveMax,ecatSendMax,ecatCustomMax);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR, "ModbusCom", "ModbusCom",1716, -1 );
        setMasterState( eMasterErr );
    }

    if(1==initial(xmlFilelist,deviceTypeList,aliasList))
    {
        initialEcatIoDidoCount();
//        isInitialedOk=true;
    }


}

ModbusCom::~ModbusCom()
{

}

int ModbusCom::getModbusDebugInfo(EthercatDebugInfo &debugInfo)
{
    //    debugInfo.slaves_al.resize(isSlaveInOp.size());
    //    for(int i=0;i<isSlaveInOp.size();i++)
    //    {
    //        debugInfo.slaves_al[i]=slavesAl[i];
    //    }
        debugInfo.masterCommandState=masterCommandState;
        debugInfo.masterCurrentState=masterCurrentState;
        debugInfo.masteroldState=masteroldState;
        debugInfo.stage=E_ETHERCAT_STAGE_INIT;
        debugInfo.wkc_count=0;
        debugInfo.wkc_lost_max=0;
        debugInfo.wkc_state=0;
        debugInfo.dcTimerShift=system_time_base;//new
        debugInfo.cycle_us=cycle_us;
        debugInfo.opLoopCount=opLoopCount;
        debugInfo.refTime=0;
        debugInfo.rerfSumTime=0;
        debugInfo.pcSumTime=0;
        debugInfo.pcCycleCount=cycle_counter;//new

        debugInfo.ecatPeriodTimerMonitor_now=ecatPeriodTimerMonitor_now;
        debugInfo.ecatPeriodTimerMonitor_max=ecatPeriodTimerMonitor_max;
        debugInfo.ecatPeriodTimerMonitor_min=ecatPeriodTimerMonitor_min;
        debugInfo.ecatReceiveMonitor_now=ecatReceiveMonitor_now;
        debugInfo.ecatReceiveMonitor_max=ecatReceiveMonitor_max;
        debugInfo.ecatReceiveMonitor_min=ecatReceiveMonitor_min;
        debugInfo.ecatSendMonitor_now=ecatSendMonitor_now;
        debugInfo.ecatSendMonitor_max=ecatSendMonitor_max;
        debugInfo.ecatSendMonitor_min=ecatSendMonitor_min;
        debugInfo.ecatCustomMonitor_now=ecatCustomMonitor_now;
        debugInfo.ecatCustomMonitor_max=ecatCustomMonitor_max;
        debugInfo.ecatCustomMonitor_min=ecatCustomMonitor_min;

        return 1;
}

int ModbusCom::initialEcatIoDidoCount()
{
    int diNum,doNum,aiNum,aoNum;
    for(int i=0;i<ioDriver.size();i++)
    {
        ioDriver[i]->getDiDoCount(diNum,doNum,aiNum,aoNum);
        if(E_MODBUS_TYPE_RTU==busType)
        {
            for(int j=0;j<robCtl->busManager->modbus_rtu_ioModule.size();j++)
            {
                if(robCtl->busManager->modbus_rtu_ioModule[j]->alias==ioDriver[i]->modbusId)
                {
                    robCtl->busManager->modbus_rtu_ioModule[j]->setCount(diNum,doNum,aiNum,aoNum);
                    break;
                }
            }
        }
        else if(E_MODBUS_TYPE_TCP==busType)
        {
            for(int j=0;j<robCtl->busManager->modbus_tcp_ioModule.size();j++)
            {
                if(robCtl->busManager->modbus_tcp_ioModule[j]->alias==ioDriver[i]->modbusId)
                {
                    robCtl->busManager->modbus_tcp_ioModule[j]->setCount(diNum,doNum,aiNum,aoNum);
                    break;
                }
            }
        }

    }
    return 1;

}



int ModbusCom::initial(QList<QString> xmlFilelist, QList<int> deviceTypeList, QList<int> aliasList)
{
    for(int i=0;i<deviceTypeList.size();i++)
    {
        if(E_EcatDev_Servo==deviceTypeList[i])
        {
            Modbus485Driver *tmpMotorDriver=
                        new Modbus485Driver(xmlFilelist[i],aliasList[i],&currentAlias,currentModbusMaster);

            motorDriver.append(tmpMotorDriver);
            motorDriverMap.insert(aliasList[i],tmpMotorDriver);
        }
        else if(E_EcatDev_IoModule==deviceTypeList[i])
        {
            ModbusIoModuleDriver *tmpIoDriver=new ModbusIoModuleDriver(xmlFilelist[i],aliasList[i],&currentAlias,currentModbusMaster);
            ioDriver.append(tmpIoDriver);
            ioDriverMap.insert(aliasList[i],tmpIoDriver);
        }


    }
    //motor
    modbus_t* tmpModbusMasterReturn;
    if(motorDriver.size()>0)
    {
        if(false==motorDriver[0]->isInitialOkStatus())
        {
            qDebug()<<"modbuscom motor initial error";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1701, -1 ,0,busType);
            setMasterState(eMasterErr);
            return -1;
        }
//        if(1!=motorDriver[0]->connectPort())
//        {
//            qDebug()<<"modbuscom motor connect error";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1702, -1 ,0,busType);
//            setMasterState(eMasterErr);
//            return -2;
//        }


        motorDriver[0]->getModbusMaster(tmpModbusMasterReturn);
        if(E_MODBUS_TYPE_RTU==busType)
        {
//            for(int i=1;i<motorDriver.size();i++)
//            {
//                motorDriver[i]->setModbusMaster(tmpModbusMasterReturn);
//                if(false==motorDriver[i]->isInitialOkStatus())
//                {
//                    qDebug()<<"modbuscom motor initial error";
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1701, -1,i ,busType);
//                    setMasterState(eMasterErr);
//                    return -3;
//                }
//            }
        }
        else if(E_MODBUS_TYPE_TCP==busType)
        {
            for(int i=1;i<motorDriver.size();i++)
            {
                if(1!=motorDriver[i]->connectPort())
                {
                    qDebug()<<"modbuscom motor connect error";
                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1702, -1 ,i,busType);
                    setMasterState(eMasterErr);
                    return -2;
                }
            }
        }
    }

    //io
    if(ioDriver.size()>0)
    {
        if(false==ioDriver[0]->isInitialOkStatus())
        {
            qDebug()<<"modbuscom io initial error";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1703, -1,0,busType );
            setMasterState(eMasterErr);
            return -4;
        }

        if(E_MODBUS_TYPE_RTU==busType)
        {

        }
        else if(E_MODBUS_TYPE_TCP==busType)
        {
            for(int i=0;i<ioDriver.size();i++)
            {
                if(false==ioDriver[i]->isInitialOkStatus())
                {
                    qDebug()<<"modbuscom io initial error";
                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1703, -1,i ,busType);
                    setMasterState(eMasterErr);
                    return -7;
                }
                int tmpReturn;
                int tryCount=0;
                do
                {
                    tmpReturn=ioDriver[i]->connectPort();
                    tryCount++;
                    if(1==tmpReturn)
                    {
                        break;
                    }
                    if(tryCount>15)
                    {
                        qDebug()<<"modbuscom io connect error";
                        addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1704, -1,i ,busType);
                        setMasterState(eMasterErr_LOCK);
                        return -8;
                    }
                    sleep(1);
                }
                while(1);

            }
        }
    }
    return 1;

}

void ModbusCom::masterError()
{
    QThread::msleep(100);
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
        if(rob->modbusRtuAxis.size()>0)
        {
            if(eServoFaultReset==rob->modbusRtuAxis.at(0)->logicMotors[0]->getMotorStateCommand())
            {
                resetMasterStatus();
            }
        }

    }

}



void ModbusCom::masterInit()
{

}

void ModbusCom::masterPreOp()
{
    isFristTimeInOp=0;
    updateControlMode();
}

void ModbusCom::masterOp()
{
    if(1<=isFristTimeInOp)
    {
        TimeTool::getInstance()->recordPeirodTime(timer_id_measure[D_TIME_TIMER]);

    }

    if(false==isInitialedOk)
    {
        sleep(2);
        isInitialedOk=true;
        isSlavesOp = true;
        if(E_MODBUS_TYPE_RTU==busType)
        {
            addMsg(ENUM_MSG_REMIND, "EcatMaster", "ModbusCom" , 1700, -1 );
        }
        else
        {
            addMsg(ENUM_MSG_REMIND, "EcatMaster", "ModbusCom" , 1715, -1 );

        }
      }


    #if D_IF_DEBUG

    TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_CUSTOM]);
    #endif
    if(true==isInitialedOk)
    {
        updateDomainData(); //读写过程数据 (帧数据交互)
    }
    #if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_CUSTOM]);

    #endif

    if(2==isFristTimeInOp)
    {
        checkTimeMaxLimit();
    }
    if(isFristTimeInOp<2)
    {
        isFristTimeInOp++;
    }
    opLoopCount++;
}

int ModbusCom::updateControlMode()
{
    int tmpReturn;
    int tryCount=0;
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
        if(E_MODBUS_TYPE_RTU==busType)
        {
            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->modbusRtuAxis.size();i++ )
                {
                    do
                    {
                        tmpReturn=motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)
                                ->setDriverMode(rob->modbusRtuAxis.at(i)->logicMotors[0]->getControlMode());
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_REMIND, "EcatMaster", "ModbusCom" , 1713, -1,tmpKey );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }
            }


        }
        else  if(E_MODBUS_TYPE_TCP==busType)
        {
            int tmpKey=writeMotorData_modbus_tcp(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->modbusTcpAxis.size();i++ )
                {
                    tryCount=0;
                    do
                    {
                        tmpReturn=motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)
                                ->setDriverMode(rob->modbusTcpAxis.at(i)->logicMotors[0]->getControlMode());
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_REMIND, "EcatMaster", "ModbusCom" , 1714, -1,tmpKey );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);
                }

            }


        }

    }
    return 1;
}


int ModbusCom::writeMotorData_modbus_rtu(RobotBase* rob)
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {

        for( int i=0;i<rob->modbusRtuAxis.size();i++ )
        {



            if(1!=motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)
                    ->setPosition(rob->modbusRtuAxis.at(i)->logicMotors[0]->targetPosition))
            {
                return -2;
            }

            //rob->modbusRtuAxis.at(i)->offset.offsetTargetTorqueSet
//            if(1!=motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)
//                    ->setMaxCurrency(rob->modbusRtuAxis.at(i)->targetTorque))
//            {
//                return -3;
//            }

            if(1!=motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)
                    ->setSpeed(rob->modbusRtuAxis.at(i)->logicMotors[0]->targetVelocity))
            {
                return -4;
            }

            if(1!=motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)
                    ->setControlWord(rob->modbusRtuAxis.at(i)->logicMotors[0]->controlWord))
            {
                return -5;
            }

//            rob->modbusRtuAxis.at(i)->targetVelocity
//            rob->modbusRtuAxis.at(i)->offset.offsetTorqueOffSet
//            rob->modbusRtuAxis.at(i)->torqueOffset
//            rob->modbusRtuAxis.at(i)->offset.offsetSpeedOffSet
//            rob->modbusRtuAxis.at(i)->velocityOffset


        }


    }
    else
    {
        //read vrep motor data

    }
    return 1;
}


int ModbusCom::writeMotorData_modbus_tcp(RobotBase* rob)
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {

        for( int i=0;i<rob->modbusTcpAxis.size();i++ )
        {



            if(1!=motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)
                    ->setPosition(rob->modbusTcpAxis.at(i)->logicMotors[0]->targetPosition))
            {
                return -2;
            }

            //rob->modbusTcpAxis.at(i)->offset.offsetTargetTorqueSet
//            if(1!=motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)
//                    ->setMaxCurrency(rob->modbusTcpAxis.at(i)->targetTorque))
//            {
//                return -3;
//            }

            if(1!=motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)
                    ->setSpeed(rob->modbusTcpAxis.at(i)->logicMotors[0]->targetVelocity))
            {
                return -4;
            }

            if(1!=motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)
                    ->setControlWord(rob->modbusTcpAxis.at(i)->logicMotors[0]->controlWord))
            {
                return -5;
            }

//            rob->modbusTcpAxis.at(i)->targetVelocity
//            rob->modbusTcpAxis.at(i)->offset.offsetTorqueOffSet
//            rob->modbusTcpAxis.at(i)->torqueOffset
//            rob->modbusTcpAxis.at(i)->offset.offsetSpeedOffSet
//            rob->modbusTcpAxis.at(i)->velocityOffset


        }


    }
    else
    {
        //read vrep motor data

    }
    return 1;
}


int ModbusCom::readMotorData_modbus_rtu(RobotBase* rob)
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {

        for( int i=0;i<rob->modbusRtuAxis.size();i++ )
        {


                int tmpReturn=0;
                if(1==motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)->getErrorCode(tmpReturn))
                {
                    rob->modbusRtuAxis.at(i)->logicMotors[0]->errorCode=tmpReturn;

                }
                else
                {
                    return -1;
                }


                if(1==motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)->getPositionFeedback(tmpReturn))
                {
                rob->modbusRtuAxis.at(i)->logicMotors[0]->actPosition = tmpReturn ;
                rob->modbusRtuAxis.at(i)->logicMotors[0]->encoderValue = rob->modbusRtuAxis.at(i)->logicMotors[0]->actPosition;
                }
                else
                {
                    return -2;
                }
                double speed;
                if(1==motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)->getSpeedFeedback(speed))
                {
                rob->modbusRtuAxis.at(i)->logicMotors[0]->writeActVelocity ( speed)  ;
                }
                else
                {
                    return -3;
                }
                double currency;
                if(1==motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)->getCurrencyFeedback(currency))
                {
                   rob->modbusRtuAxis.at(i)->logicMotors[0]->actTorque = currency ;
                }
                else
                {
                    return -4;
                }


                if(1==motorDriverMap.value(rob->modbusRtuAxis.at(i)->alias)->getStatusWord(tmpReturn))
                {
                    rob->modbusRtuAxis.at(i)->logicMotors[0]->stateWord = tmpReturn ;
                }
                else
                {
                    return -5;
                }

                //rob->modbusRtuAxis.at(i)->actFollowErr =  ;
                //rob->modbusRtuAxis.at(i)->disModeOfOperation = ;
                //rob->modbusRtuAxis.at(i)->digIn = ;
                //rob->modbusRtuAxis.at(i)->digOut =  ;


        }

        rob->checkModbusRtuDeviceError();

    }
    else
    {
        //read vrep motor data
        return -100;

    }
    return 1;
}

int ModbusCom::readMotorData_modbus_tcp(RobotBase* rob)
{


    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {

        for( int i=0;i<rob->modbusTcpAxis.size();i++ )
        {


                int tmpReturn=0;
                if(1==motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)->getErrorCode(tmpReturn))
                {
                    rob->modbusTcpAxis.at(i)->logicMotors[0]->errorCode=tmpReturn;

                }
                else
                {
                    return -1;
                }

                //rob->modbusRtuAxis.at(i)->stateWord =   ;
                if(1==motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)->getPositionFeedback(tmpReturn))
                {
                rob->modbusTcpAxis.at(i)->logicMotors[0]->actPosition = tmpReturn ;
                rob->modbusTcpAxis.at(i)->logicMotors[0]->encoderValue = rob->modbusTcpAxis.at(i)->logicMotors[0]->actPosition;
                }
                else
                {
                    return -2;
                }
                double speed;
                if(1==motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)->getSpeedFeedback(speed))
                {
                rob->modbusTcpAxis.at(i)->logicMotors[0]->writeActVelocity ( speed ) ;
                }
                else
                {
                    return -3;
                }

                double currency;
                if(1==motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)->getCurrencyFeedback(currency))
                {
                   rob->modbusTcpAxis.at(i)->logicMotors[0]->actTorque = currency ;
                }
                else
                {
                    return -4;
                }


                if(1==motorDriverMap.value(rob->modbusTcpAxis.at(i)->alias)->getStatusWord(tmpReturn))
                {
                    rob->modbusTcpAxis.at(i)->logicMotors[0]->stateWord = tmpReturn ;
                }
                else
                {
                    return -5;
                }

                //rob->modbusRtuAxis.at(i)->actFollowErr =  ;
                //rob->modbusRtuAxis.at(i)->disModeOfOperation = ;
                //rob->modbusRtuAxis.at(i)->digIn = ;
                //rob->modbusRtuAxis.at(i)->digOut =  ;


        }

        rob->checkModbusTcpDeviceError();

    }
    else
    {
        //read vrep motor data
        return -100;

    }
    return 1;
}


int ModbusCom::readMotorData(RobotBase* rob)
{
    //read　real motor data
    if(E_MODBUS_TYPE_RTU==busType)
    {
        int tmpKey=readMotorData_modbus_rtu(rob);
        if(1!=tmpKey)
        {
            return -1;
        }
    }
    else if(E_MODBUS_TYPE_TCP==busType)
    {
        int tmpKey=readMotorData_modbus_tcp(rob);
        if(1!=tmpKey)
        {
            return -1;
        }
    }

    return 1;
}


int ModbusCom::writeMotorData(RobotBase* rob)
{
    //write　real motor data
    if(E_MODBUS_TYPE_RTU==busType)
    {
        int tmpKey=writeMotorData_modbus_rtu(rob);
        if(1!=tmpKey)
        {
            return -1;
        }
    }
    else if(E_MODBUS_TYPE_TCP==busType)
    {
        int tmpKey=writeMotorData_modbus_tcp(rob);
        if(1!=tmpKey)
        {
            return -1;
        }
    }

    return 1;
}

int ModbusCom::readIoData_modbus_rtu(RobotBase *rob)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //read real io_module data
       for( int i=0;i<rob->modbusRtuIoModule.size();i++ )
       {

           QVector<bool> tmpDi;
           int tmpKey=ioDriverMap.value(rob->modbusRtuIoModule[i]->alias)->readAllDi(tmpDi);
           if(1!=tmpKey)
           {
               return -1;

           }
           for(int j=0;j<tmpDi.size();j++)
           {
                rob->modbusRtuIoModule[i]->modbus_diMemory[j]=tmpDi[j];
           }
       }

   }
   else
   {
       //read vrep io
       // todo
   }
    return 1;
}


int ModbusCom::readIoData_modbus_tcp(RobotBase *rob)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //read real io_module data
       for( int i=0;i<rob->modbusTcpIoModule.size();i++ )
       {

           QVector<bool> tmpDi;
           int tmpKey=ioDriverMap.value(rob->modbusTcpIoModule[i]->alias)->readAllDi(tmpDi);
           if(1!=tmpKey)
           {
               return -1;
           }
           for(int j=0;j<tmpDi.size();j++)
           {
                rob->modbusTcpIoModule[i]->modbus_diMemory[j]=tmpDi[j];
           }
       }

   }
   else
   {
       //read vrep io
       // todo
   }
    return 1;
}


int ModbusCom::readIoData(RobotBase* rob)
{
    //read　real io data
    if(E_MODBUS_TYPE_RTU==busType)
    {
        return readIoData_modbus_rtu(rob);

    }
    else if(E_MODBUS_TYPE_TCP==busType)
    {
        return readIoData_modbus_tcp(rob);

    }


    return -1;


}


int ModbusCom::writeIoData_modbus_tcp(RobotBase *rob)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //write real io_module data
       for( int i=0;i<rob->modbusTcpIoModule.size();i++ )
       {

           QVector<bool> tmpDo;
           int doSize=rob->modbusTcpIoModule[i]->doCount;
           tmpDo.resize(doSize);
           for(int k=0;k<rob->modbusTcpIoModule[i]->doCount;k++)
           {
               tmpDo[k]=rob->modbusTcpIoModule[i]->modbus_doMemory[k];
           }

           int tmpKey=ioDriverMap.value(rob->modbusTcpIoModule[i]->alias)->writeDo(tmpDo);
           if(1!=tmpKey)
           {
               return -1;
           }

       }

   }
   else
   {
       //write vrep io
       // todo
        return -100;
   }
    return 1;
}



int ModbusCom::writeIoData_modbus_rtu(RobotBase *rob)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //write real io_module data
       for( int i=0;i<rob->modbusRtuIoModule.size();i++ )
       {

           QVector<bool> tmpDo;
           tmpDo.resize(rob->modbusRtuIoModule[i]->doCount);
           for(int k=0;k<rob->modbusRtuIoModule[i]->doCount;k++)
           {
               tmpDo[k]=rob->modbusRtuIoModule[i]->modbus_doMemory[k];
           }

           int tmpKey=ioDriverMap.value(rob->modbusRtuIoModule[i]->alias)->writeDo(tmpDo);
           if(1!=tmpKey)
           {
               return -1;
           }

       }

   }
   else
   {
       //write vrep io
       // todo
        return -100;
   }
    return 1;
}


int ModbusCom::writeIoData(RobotBase* rob)
{
    //read　real io data
    if(E_MODBUS_TYPE_RTU==busType)
    {
        writeIoData_modbus_rtu(rob);

    }
    else if(E_MODBUS_TYPE_TCP==busType)
    {
        writeIoData_modbus_tcp(rob);

    }


    return 1;


}

int ModbusCom::checkTimeMaxLimit()
{

    S_TIME_DATA* dataReceive;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_RECEIVE],&dataReceive);
    S_TIME_DATA* dataCustom ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_CUSTOM],&dataCustom);
    S_TIME_DATA* dataSend ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_SEND],&dataSend);
    S_TIME_DATA* dataPeriodTimer ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_TIMER],&dataPeriodTimer);

    int a, b, c,e,f,g;
    e=timer_id_measure[D_TIME_RECEIVE];
    f=timer_id_measure[D_TIME_CUSTOM];
    g=timer_id_measure[D_TIME_SEND];
    a=dataReceive->maxTime;
    b=dataSend->maxTime;
    c=dataCustom->maxTime;

    ecatPeriodTimerMonitor_now=dataPeriodTimer->nowTime;
    ecatPeriodTimerMonitor_max=dataPeriodTimer->maxTime;
    ecatPeriodTimerMonitor_min=dataPeriodTimer->minTime;
    ecatReceiveMonitor_now=dataReceive->nowTime;
    ecatReceiveMonitor_max=dataReceive->maxTime;
    ecatReceiveMonitor_min=dataReceive->minTime;
    ecatSendMonitor_now=dataSend->nowTime;
    ecatSendMonitor_max=dataSend->maxTime;
    ecatSendMonitor_min=dataSend->minTime;
    ecatCustomMonitor_now=dataCustom->nowTime;
    ecatCustomMonitor_max=dataCustom->maxTime;
    ecatCustomMonitor_min=dataCustom->minTime;



    if(dataPeriodTimer->nowTime>cycle_ns+ecatPeriodTimerOffsetMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9004, -1, dataPeriodTimer->nowTime
//               ,cycle_ns+ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
        timerErrorCount++;

    }
    else if(dataPeriodTimer->nowTime<cycle_ns-ecatPeriodTimerOffsetMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9003, -1, dataPeriodTimer->nowTime
//               ,cycle_ns-ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
        timerErrorCount++;

    }
    else
    {
        timerErrorCount=0;
    }
    if(timerErrorCount>4)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1717, -1, dataPeriodTimer->nowTime
                       ,cycle_ns+ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
    }


    if(dataReceive->nowTime>ecatReceiveMax)//300us
    {
        receiveErrorCount++;
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9302, -1, dataReceive->nowTime,ecatReceiveMax );

    }
    else
    {
        receiveErrorCount=0;
    }
    if(receiveErrorCount>4)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1718, -1, dataReceive->nowTime,ecatReceiveMax );
    }

    if(dataSend->nowTime>ecatSendMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9303, -1, dataSend->nowTime,ecatSendMax );
        sendErrorCount++;
    }
    else
    {
        sendErrorCount=0;
    }
    if(sendErrorCount>4)
    {
       addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1719, -1, dataSend->nowTime,ecatSendMax );
    }

    //if(dataCustom->maxTime>800000)//800us
    if(dataCustom->nowTime >  ecatCustomMax)//超过80%报警
    {
        customErrorCount++;
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9304, -1, dataCustom->nowTime, ecatCustomMax);
//        printf("error : ethercat custom time exceed limit------------------------------");
    }
    else
    {
        customErrorCount=0;
    }
    if(customErrorCount>4)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1720, -1, dataCustom->nowTime, ecatCustomMax);
    }

    return 1;
}


void ModbusCom::updateDomainData()
{

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob ){
            continue;
        }
//        if( !rob->isLinkOk )
//            continue;

//        QThread::msleep(50);//以前综科智能控制的modbus-tcp模块需要这里休息50ms

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_RECEIVE]);


        int keyReturn;

        //read motor
        static int tryCount_readMotor=0;
        keyReturn = readMotorData(rob);
        if(1!=keyReturn)
        {
            tryCount_readMotor++;
        }
        else
        {
            tryCount_readMotor=0;
        }
        if(tryCount_readMotor>0)
        {
            qDebug()<<"readMotorData tryCount_readMotor="<<tryCount_readMotor;
        }
        if(tryCount_readMotor>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, modbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1705, -1,keyReturn );
        }


        //read io
        static int tryCount_readIo=0;
        keyReturn = readIoData(rob);
        if(1!=keyReturn)
        {
            tryCount_readIo++;
        }
        else
        {
            tryCount_readIo=0;
        }
        if(tryCount_readIo>0)
        {
            qDebug()<<"readIoData tryCount_readIo="<<tryCount_readIo;
        }
        if(tryCount_readIo>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, modbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1707, -1,keyReturn );
        }



        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_RECEIVE]);


        if(E_MODBUS_TYPE_RTU==busType)
        {
            rob->updateInputModbusRtuIodata();//io数据映射交换
            rob->updateOutputModbusRtuIodata();
        }
        else if(E_MODBUS_TYPE_TCP==busType)
        {
            rob->updateInputModbusTcpIodata();//io数据映射交换
            rob->updateOutputModbusTcpIodata();
            QThread::msleep(50);
        }

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_MATH]);
        setServoData(rob);//算法数据交换
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_MATH]);


        if(E_MODBUS_TYPE_RTU==busType)
        {
            rob->checkModbusRtuDeviceError();
            for( int i=0;i<rob->modbusRtuAxis.size();i++ )
            {
                rob->modbusRtuAxis.at(i)->updateMotorState();
            }
        }
        else if(E_MODBUS_TYPE_TCP==busType)
        {
            rob->checkModbusTcpDeviceError();
            for( int i=0;i<rob->modbusTcpAxis.size();i++ )
            {
                rob->modbusTcpAxis.at(i)->updateMotorState();
            }
        }



        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_SEND]);



        //write motor
        static int tryCount_writeMotor=0;
        keyReturn = writeMotorData(rob);
        if(1!=keyReturn)
        {
            tryCount_writeMotor++;
        }
        else
        {
            tryCount_writeMotor=0;
        }
        if(tryCount_writeMotor>0)
        {
            qDebug()<<"writeMotorData tryCount_writeMotor="<<tryCount_writeMotor;
        }
        if(tryCount_writeMotor>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, modbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1711, -1,keyReturn );
        }


        //write io
        static int tryCount_writeIo=0;
        keyReturn = writeIoData(rob);
        if(1!=keyReturn)
        {
            tryCount_writeIo++;
        }
        else
        {
            tryCount_writeIo=0;
        }
        if(tryCount_writeIo>0)
        {
            qDebug()<<"writeIoData tryCount_writeIo="<<tryCount_writeIo;
        }
        if(tryCount_writeIo>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, modbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "ModbusCom" , 1708, -1,keyReturn );
        }

        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_SEND]);


    }
}


void ModbusCom::addMsg(int messageLevel,
                            std::string componentName,
                            std::string messageType,
                            int messageCode,
                            int robotId,
                            int parameter1 ,
                            int parameter2 ,
                            int parameter3 ,
                            int parameter4 )
{
//    struct timeval tv_begin ;
//    gettimeofday(&tv_begin, NULL);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
//    tmpMsg.time = tv_begin.tv_sec;

    QString infomationStr;
    switch(messageCode)
    {
    case 1720:
    {
        infomationStr =  QObject::tr("modbus custom time(%1) exceed limit(%2 ns) busType=(%3) !")
                .arg(parameter1).arg(parameter2).arg((int)busType);
    }
        break;
    case 1719:
    {
        infomationStr =  QObject::tr("modbus send time(%1) exceed limit(%2 ns) busType=(%3) !")
                .arg(parameter1).arg(parameter2).arg((int)busType);

    }
        break;
    case 1718:
    {
        infomationStr =  QObject::tr("modbus receive time(%1 ns) exceed limit(%2 ns) busType=(%3) !")
                .arg(parameter1).arg(parameter2).arg((int)busType);
    }
        break;
    case 1717:
    {
        infomationStr =  QObject::tr("modbus period timer(%1 ns) exceed  limit(%2 ns),cycle_ns=%3,cycle_us=%4  busType=(%5) !")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4).arg((int)busType);

    }
        break;
    case 1716:
    {
        infomationStr =  QObject::tr("读取checkConfig.ini文件失败 !busType==%1").arg((int)busType);
        break;

    }
    case 1715:
    {
        infomationStr =  QObject::tr("系统初始化完成，请您操作(modbus_tcp) !busType==%1").arg((int)busType);
        break;

    }
    case 1714:
    {
        infomationStr =  QObject::tr("modbus tcp motor设备操作模式设置失败 !");
        break;

    }
    case 1713:
    {
        infomationStr =  QObject::tr("modbus rtu motor设备操作模式设置失败 !");
        break;

    }
    case 1712:
    {
        infomationStr =  QObject::tr("modbus tcp motor设备写操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1711:
    {
        infomationStr =  QObject::tr("modbus rtu motor设备写操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1710:
    {
        infomationStr =  QObject::tr("modbus tcp io写备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1709:
    {
        infomationStr =  QObject::tr("modbus rtu io写备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1708:
    {
        infomationStr =  QObject::tr("modbus tcp io设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1707:
    {
        infomationStr =  QObject::tr("modbus rtu io设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1706:
    {
        infomationStr =  QObject::tr("modbus tcp motor设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1705:
    {
        infomationStr =  QObject::tr("modbus rtu motor设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }

    case 1704:
    {
        infomationStr =  QObject::tr("modbus io 设备连接不上,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1703:
    {
        infomationStr =  QObject::tr("modbus io 设备配置文件错误,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1702:
    {
        infomationStr =  QObject::tr("modbus motor 设备连接不上,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1701:
    {
        infomationStr =  QObject::tr("modbus motor 设备配置文件错误,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }

    case 1700:
    {
        infomationStr =  QObject::tr("系统初始化完成，请您操作(modbus_rtu) !busType==%1").arg((int)busType);
        break;

    }

    }
    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}

